#!/usr/bin/env python

import sys, os
import math
import rospy
from sensor_msgs.msg import Image
from vision_messages.srv import FaceDetect
import numpy as np
import cv2
import message_filters

class Face_Client():
    def __init__(self):
        self.get_image()

    def client(self):
        rospy.wait_for_service('/perception/face_detector')
        try:
            get_face = rospy.ServiceProxy('/perception/face_detector', FaceDetect)
            result = get_face(self.image, self.depth)
            return result
        except rospy.ServiceException as e:            
            rospy.loginfo("Perception Service call failed: %s"%e)

    def get_image(self):
        image_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
        depth_sub = message_filters.Subscriber('/camera/aligned_depth_to_color/image_raw', Image)
        ts = message_filters.TimeSynchronizer([image_sub, depth_sub], 100)
        ts.registerCallback(self.callback)
        
        # rospy.Subscriber("/camera/color/image_raw", Image, self.image_callback)
        self.image = rospy.wait_for_message("/camera/color/image_raw", Image)

        # rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, selfs.depth_callback)
        self.depth = rospy.wait_for_message("/camera/aligned_depth_to_color/image_raw", Image)

        # print(self.image.header.stamp)
        # print(self.depth.header.stamp)

    def callback(self, image, depth):
        # rospy.loginfo('Get Image and Depth!')
        # self.image = image
        # self.depth = depth
        pass
        # print(self.image.header.stamp)
        # print(self.depth.header.stamp)

    def image_callback(self, data):
        self.image = data

    def depth_callback(self, data):
        self.depth = data

if __name__ == "__main__":
    rospy.init_node('percenption_face_client')
    image_pub = rospy.Publisher('/perception/face_detector/face_mask', Image, queue_size = 1)
    while True:
        percenption = Face_Client()
        result = percenption.client()
        if len(result.lip_point) == 0:
            rospy.loginfo('WARNING! No Object detected! Please check again!')
            # continue
        # vis
        ros_frame = result.frame
        image_pub.publish(ros_frame)
        # rate = rospy.Rate(10)
        rospy.loginfo("Get Face Result!")
        # break